/*
 *    Copyright (c) 2014 Xiang Xu <xuxiang@mail.bnu.edu.cn>
 *
 *    Permission is hereby granted, free of charge, to any person
 *    obtaining a copy of this software and associated documentation
 *    files (the "Software"), to deal in the Software without
 *    restriction, including without limitation the rights to use,
 *    copy, modify, merge, publish, distribute, sublicense, and/or sell
 *    copies of the Software, and to permit persons to whom the
 *    Software is furnished to do so, subject to the following
 *    conditions:
 *
 *    The above copyright notice and this permission notice shall be
 *    included in all copies or substantial portions of the Software.
 *
 *    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 *    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 *    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 *    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 *    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 *    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 *    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 *    OTHER DEALINGS IN THE SOFTWARE.
 */

#include "arguments.h"
#include "pclwraper.h"

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/transforms.h>

// cloud viewer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

// query this after executing a function
Status status;

// organize variables
boost::shared_ptr<Arguments> arguments = boost::shared_ptr<Arguments>(new Arguments);

// raw building cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr build_cloud_raw(new pcl::PointCloud<pcl::PointXYZ>);
// transformed raw cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr build_cloud_transformed_raw(new pcl::PointCloud<pcl::PointXYZ>);

// raw building plane
pcl::PointCloud<pcl::PointXYZ>::Ptr build_cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);
// transformed building plane
pcl::PointCloud<pcl::PointXYZ>::Ptr build_cloud_transformed_plane(new pcl::PointCloud<pcl::PointXYZ>);

// search tree
pcl::search::KdTree<pcl::PointXYZ> search;

// only allow to transform once
bool transform = true;

// to select plane original point and normal vector
int pick_time = 0;
std::vector<pcl::PointXYZ> picked_points;

// of build plane
Eigen::VectorXf build_plane_coeff(4);
// spatial extent of build plane
pcl::PointXYZ min_extent, max_extent;

// Functions
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie);
void FindPickedPoint(const pcl::visualization::PointPickingEvent& event);
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* cookie);
void TransformCloud();
void Create_building_plane_bounder_box_and_add_to_viewer();
void ShowCloud();

int main(int argc, char** argv)
{
    // parse parameters
    status = arguments->Parse(argc, argv);

    if(!status.IsOk())
    {
        cout << status;
        exit(-1);
    }
    else
    {
        cout << "Argments Parsed success.\n";
    }

    // fetch parameters
    double threshold = arguments->GetPlaneThreshold();

    if(threshold == -1)
    {
        cout << "Plane threshold not set! Please set \"plane_distance\" option in configure.json file.\n";
    }

    // load point cloud
    PclWraper::LoadPCD(arguments->GetFacadeFile(), build_cloud_raw);

    PclWraper::ExtractPlane(build_cloud_raw, build_cloud_plane, threshold, build_plane_coeff);

    // Event loop ...
    ShowCloud();
}

void ShowCloud()
{
    viewer.reset(new pcl::visualization::PCLVisualizer());
    viewer->addPointCloud<pcl::PointXYZ> (build_cloud_plane, "build plane cloud");
    viewer->setBackgroundColor(0, 0, 0);
    viewer->registerPointPickingCallback(&pp_callback);
    viewer->registerKeyboardCallback(&keyboardEventOccurred);
    viewer->spin();
}

void FindPickedPoint(const pcl::visualization::PointPickingEvent& event)
{
    int idx = event.getPointIndex();

    if(idx == -1)
    {
        std::cout << "Invalid pick!\n;";
        return;
    }

    search.setInputCloud(build_cloud_plane);

    // Return the correct index in the cloud instead of the index on the screen
    std::vector<int> indices(1);
    std::vector<float> distances(1);

    // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
    pcl::PointXYZ picked_pt;
    event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
    search.nearestKSearch(picked_pt, 1, indices, distances);
    picked_points.push_back(picked_pt);
}

void pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie)
{
    if(pick_time == 0)
    {
        viewer->removeAllShapes();
        picked_points.clear();

        FindPickedPoint(event);

        pick_time = 1;

        cout << "Original point Chosen! [" << picked_points[0].x << ", "
             << picked_points[0].y << ", " << picked_points[0].z << "]\n";
    }
    else if(pick_time == 1)
    {
        FindPickedPoint(event);

        pick_time = 2;

        cout << "First point chosen![" << picked_points[1].x << ", "
             << picked_points[1].y << ", " << picked_points[1].z << "]\n";
    }
    else
    {
        FindPickedPoint(event);
        viewer->addArrow(picked_points[2], picked_points[1], 1.0, 0.0, 0.0, "arrow");

        pick_time = 0;

        cout << "Second point chosen! And it's ready to transform.[" << picked_points[2].x << ", "
             << picked_points[2].y << ", " << picked_points[2].z << "]\n";
    }
}

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* cookie)
{
    //std::cout << event.getKeySym();

    if(event.getKeySym() == "t" && transform)
    {
        cout << "Transform ... \n";
        TransformCloud();
        cout << "Transform Done.\n";

        transform = false;
    }

    if(event.getKeySym() == "o")
    {
        pick_time = 0;
    }
}

void TransformCloud()
{
    /// get the original point, X and Y direction normal of the new coordinate
    Eigen::Affine3f transform;
    Eigen::Vector3f y_direction, z_axis, origin;
    origin(0) = picked_points[0].x;
    origin(1) = picked_points[0].y;
    origin(2) = picked_points[0].z;
    y_direction(0) = picked_points[2].x - picked_points[1].x;
    y_direction(1) = picked_points[2].y - picked_points[1].y;
    y_direction(2) = picked_points[2].z - picked_points[1].z;
    z_axis(0) = build_plane_coeff(0);
    z_axis(1) = build_plane_coeff(1);
    z_axis(2) = build_plane_coeff(2);
    y_direction.normalize();
    z_axis.normalize();

    double result2 = y_direction(0) * z_axis(0) + y_direction(1) * z_axis(1)
                     + y_direction(2) * z_axis(2);
    cout << "Test orthogonal: " << result2 << endl;

    // Get transformation matrix
    pcl::getTransformationFromTwoUnitVectorsAndOrigin(y_direction,
            z_axis, origin, transform);

    // transform the cloud
    pcl::transformPointCloud(*build_cloud_plane,
                             *build_cloud_transformed_plane, transform);

    pcl::transformPointCloud(*build_cloud_raw,
                             *build_cloud_transformed_raw, transform);

    // save...
    PclWraper::SavePCD(arguments->GetFacadePlaneTransformedFile(), build_cloud_transformed_plane);
    cout << arguments->GetFacadePlaneTransformedFile() << " is saved.\n";

    PclWraper::SavePCD(arguments->GetFacadeTransformedFile(), build_cloud_transformed_raw);

    cout << arguments->GetFacadeTransformedFile() << " is saved.\n";

    pcl::PointXYZ min_extent, max_extent;
    pcl::getMinMax3D(*build_cloud_transformed_plane, min_extent, max_extent);

    // save facade scale
    arguments->SaveSpatialScale(min_extent.x, min_extent.y, min_extent.z, max_extent.x,
                                max_extent.y, max_extent.z);

    viewer->updatePointCloud(build_cloud_transformed_plane, "build plane cloud");
    viewer->initCameraParameters();
    viewer->removeAllShapes();

    // get the bounder box of the transformed cloud
    Create_building_plane_bounder_box_and_add_to_viewer();
}

void Create_building_plane_bounder_box_and_add_to_viewer()
{
    // get the bounder box of the transformed cloud
    pcl::getMinMax3D(*build_cloud_transformed_plane, min_extent, max_extent);

    pcl::PointCloud<pcl::PointXYZ>::Ptr build_polygon_points(new pcl::PointCloud<pcl::PointXYZ>);
    build_polygon_points->width = 4;
    build_polygon_points->height = 1;
    pcl::PointXYZ p1(min_extent.x, min_extent.y, 0.0), p2(min_extent.x, max_extent.y, 0.0),
        p3(max_extent.x, max_extent.y, 0.0), p4(max_extent.x, min_extent.y, 0.0);

    build_polygon_points->points.push_back(p1);
    build_polygon_points->points.push_back(p2);
    build_polygon_points->points.push_back(p3);
    build_polygon_points->points.push_back(p4);

    Eigen::Vector4f planarCoeff;
    planarCoeff(0) = 0.0;
    planarCoeff(1) = 0.0;
    planarCoeff(2) = 1.0;
    planarCoeff(3) = 0.0;
    pcl::PlanarPolygon<pcl::PointXYZ> planarPolygon(build_polygon_points->points, planarCoeff);

    // And add the bounder box to the viewer
    const std::string facadeId = "facade";
    viewer->addPolygon(planarPolygon, 0.0, 0.0, 1.0, facadeId);

    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
                                        pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, facadeId);
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.8, facadeId);

    cout << "\nSummary: \n";
    cout << "Facade Extent: [ minx, miny, minz, maxx, maxy, maxz] == [ " <<
         min_extent.x << ", " << min_extent.y << ", " <<  min_extent.z << ", "
         << max_extent.x << ", " << max_extent.y << ", " << max_extent.z << " ]" << endl;
}

